import cv2
import numpy as np
import RPi.GPIO as GPIO
import time
# ====== 舵机初始化 ======
SERVO_PIN = 27  # 舵机信号引脚 GPIO27
GPIO.setmode(GPIO.BCM)  # 使用 BCM 编号
GPIO.setup(SERVO_PIN, GPIO.OUT)
# 创建 PWM 实例，频率为 50Hz（舵机标准频率）
pwm = GPIO.PWM(SERVO_PIN, 50)
pwm.start(0)  # 初始占空比为 0
def set_servo_angle(angle):
    """设置舵机角度"""
    duty = angle / 18 + 2  # 将角度转换为占空比
    GPIO.output(SERVO_PIN, True)
    pwm.ChangeDutyCycle(duty)
    time.sleep(0.5)  # 等待舵机转动到目标位置
    GPIO.output(SERVO_PIN, False)
    pwm.ChangeDutyCycle(0)
# 舵机的默认角度和检测到人脸时的角度
DEFAULT_ANGLE = 0  # 默认位置
DETECTED_ANGLE = 90  # 检测到人脸时的目标位置
# 当前舵机状态
current_angle = DEFAULT_ANGLE  # 默认位置
# ====== 人脸检测初始化 ======
face_cascade = cv2.CascadeClassifier(cv2.data.haarcascades + 'haarcascade_frontalface_default.xml')
# 检查模型是否加载成功
if face_cascade.empty():
    print("错误：人脸检测模型未加载")
    exit()
# 加载训练好的识别模型和标签
recognizer = cv2.face.LBPHFaceRecognizer.create()
recognizer.read('/home/hg/p/face_model.yml')
label_dict = np.load('label_dict.npy', allow_pickle=True).item()
# 打开摄像头
cap = cv2.VideoCapture(0)
try:
    while True:
        ret, frame = cap.read()
        if not ret:
            break
        # 转换为灰度图
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        # 检测人脸
        faces = face_cascade.detectMultiScale(
            gray,
            scaleFactor=1.2,
            minNeighbors=5,
            minSize=(50, 50)
        )
        # 如果检测到人脸且舵机未处于目标位置，转动舵机
        if len(faces) > 0 and current_angle != DETECTED_ANGLE:
            set_servo_angle(DETECTED_ANGLE)
            current_angle = DETECTED_ANGLE  # 更新舵机状态
            
        # 如果未检测到人脸且舵机未处于默认位置，转回默认位置
        elif len(faces) == 0 and current_angle != DEFAULT_ANGLE:
            set_servo_angle(DEFAULT_ANGLE)
            current_angle = DEFAULT_ANGLE  # 更新舵机状态

        # 识别并标注
        for (x, y, w, h) in faces:
            face_roi = gray[y:y + h, x:x + w]
            face_resized = cv2.resize(face_roi, (100, 100))  # 与训练尺寸一致
            label, confidence = recognizer.predict(face_resized)
            name = label_dict.get(label, "Unknown")
            # 绘制结果
            cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2)
            cv2.putText(frame, f"{name} ({confidence:.2f})", (x, y - 10),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
        cv2.imshow('Real-time Face Recognition', frame)
        if cv2.waitKey(1) == ord('q'):
            break
finally:
    # 释放资源
    cap.release()
    cv2.destroyAllWindows()
    pwm.stop()
    GPIO.cleanup()